BACKGROUND OF THE INVENTION 



1 . Field of the Invention 

This invention pertains in general to a system and method for the accurate estimation of 
the position of and underwater vehicle and more particularly to a system and method for 
the estimation and tracking of an underwater vehicle's position using a combination of 
bathymetry data, and the underwater vehicles dynamics data in relation to a position 
marker. 

2. Description of the Prior Art 

Under water vehicles are in common use for surveying and locating objects in the ocean 
due to their relatively low operation cost. To be useful however, the position of an 
underwater vehicle must be known so that the data collected by the towed vehicle's 
sensors can be georectified. Due to ocean dynamics, currents, waves, temperature 
fluctuation and etc., acting on underwater vehicle's tether or on the vehicle itself, a 
submerged vehicles position relative to the towing vehicle or some other georectified 
point is often difficult to determine. Since the ocean is opaque to high-frequency 
electromagnetic signals, the global positioning system (GPS) generally cannot be used for 
submerged vehicle positioning. 

Current methods for determining the position of a underwater vehicle include the lay- 
back model (See, FIG. 1), the long base line (LBL) model (See FIG. 2), the short base 
line (SBL) model (See FIG. 3), the inverted SBL system and the Localization System 
(LOST)(See FIG. 4). These system are generally configured to manage the position of a 
tethered or towed vehicle and are ill equipped for an remote or untethered underwater 
vehicle. 

With the lay-back model method the underwater vehicle is tethered and is assumed to be 
directly behind the towing vehicle and the lay-back (distance from the towing vehicle) is 
assumed to be a fixed multiple of the tow vehicle's depth. Alternatively, the amount of 
cable paid out is assumed to be the slant range to the tow vehicle. The inverted SLB 
method does not take into account the cable's cantilever. The neither the lay-back, the 
LBL, or the SLB methods take in account the effects of local current on the cables' shape 
and position. The accuracy of these methods will in general suffer as a function of time 
and therefore not support precise positioning of the towed vehicle. 

The LBL method uses a series of acoustic transponders to localized the tow-vehicle by 
measuring the time delays between the tow -vehicle and the transponders. These 
transponders may be bottom mounted or locaHzed on the surface, such as shown in U.S. 
Patent 5,1 19,341. While these systems are very accurate, their deployment is a time 
consuming and expensive operation. Furthermore the LBL method requires that the 
systems be redeployed to each operational area. 
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The SBL method uses time delay and measured arrival angle of an acoustic signal from 
the tow- vehicle to the tov^ing vehicle to compute position. These systems are fairly 
accurate but their resolution is range dependent. Typically, in order to achieve sufficient 
accuracy, a SBL system is deployed over the tow-vehicle, requiring a second surface 
craft-an obvious economic disadvantage. 

With the inverted SBL method, the directional acoustic receiver is mounted on the towed 
vehicle instead of the towing vehicle. As a consequence the range dependent accuracy of 
this system is a problem for very deep water operation. SBL and inverted SBL systems 
cannot be readily used in applications other than ship towed systems. 

The LOST 1, localization method, the position of a towed underwater vehicle is 
generated via the use of bathymetry data collected by the towing vessel and the depth 
range of the underwater vehicle from the towing vessel as shovra in U.S. Patent 
6,256,264 incorporated herein by reference. LOST 1 uses a single dimensional model to 
generate the underwater vehicle's position assuming the underwater vehicles position 
along track doesn't change. This feature limits the deployment of the underwater vehicle 
to an area directly behind the tow vessel in order to achieve sufficient accuracy. 

Summary 

A system for the accurate determination of the position of an underwater vehicle 
comprising a system observer subsystem having a state velocity update module, a terrain 
matching module, means for generating a prediction of the terrain matching module's 
performance and a constrained extended Kalman filter subsystem. The constrained 
extended Kalman filter subsystem includes a steady state extended Kalman filter, a non- 
linear constraint module, and a state predictor. The system observer integrates 
bathymetry data corresponding to the area of the submersible vehicle, with the vessel's 
measured ocean depth, the vessel's predicted state, the vessel's measured velocity into a 
terrain based state estimate, a final predicted state, the Kalman filter takes the terrain 
based state estimate, the final predicted state, the measured slant range and the location of 
the known point and computes the final estimate of the vessel's position and a prediction 
of the vessel's position at the next time step. A method for the accurate determination of 
the position of at least one underwater vehicle comprising the steps of (1) acoustically 
coupling at least one underwater vehicle to a sea borne position marker having a known 
position; (2) predicting the at least one underwater vehicle's position, based on a past 
estimate of the underwater vehicle's position, and an estimate of its velocity over the sea 
bottom; (3) estimating the underwater vehicle's position ufihzing measured ocean depth 
at the underwater vehicle's position, bathymetry data and the underwater vehicle's 
predicted position in a single point terrain match; (4) computing a estimate of the 
underwater vehicle's position based on the prediction of the at least one underwater 
vehicle's position based on vehicle dynamics and the estimated underwater vehicles 
position based on ocean depth and bathymetry data; and (5) computing a corrected 
estimate of the at least one submersible vehicle's position that utihzes the estimate of the 
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underwater vehicle's position and a measured slant range from the at least one 
submersible vehicle to the sea borne position marker whose position is known. 

BRIEF DESCRIPTION OF THE DRAWINGS 

Figure 1 shows a cable lay back method of the prior art. 

Figure 2 shows a long base line (LBL) approach of the prior art. 

Figure 3 shows a short base line (SBL) approach of the prior art. 

Figure 4 shows a localization system (LOST 1) approach of the prior art. 

Figure 5 shows an overview of the implementation of the LOST 2 system employing a 

floating buoy position marker. 

Figure 6 shows a top view of the LOST 2 system employing multiple underwater 
vehicles. 

Figure 7 shows an example implementation of the LOST 2 system employing a position 

marker anchored on the sea floor. 

Figure 8 shows a block diagram of the LOST 2 system. 

Figure 9 shows a block diagram of the system observer subsystem. 

Figure 10 shows a block diagram of the signal flow of the state velocity update module. 

Figure 1 1 shows a block diagram of the signal flow of the terrain matching module. 

Figure 12 shows a block diagram of the signal flow for the spatial based performance 

prediction module. 

Figure 13 shows a block diagram of constrained extended Kalman filter sub-system. 
Figure 14 shows a block diagram of the signal flow for the steady state Kalman filter. 
Figure 15 shows a block diagram of the signal flow for nonlinear constraint module. 
Figure 16 shows a block diagram of the signal flow for the state predictor module. 

Detailed Description 

The LOST-2 underwater positioning system is a system for the accurate determination of 
the position of an underwater vehicle. The system and method combines parts of 
dead-reckoning, acoustic-based, and terrain-based positioning into a single integrated 
system and apply these various methods in an underwater environment. The system is 
designed in two basic subsystems, the system observer and the constrained extended 
Kalman filter. Prior to discussing the figures a brief description of the basic architecture 
of each subsystem follows, however the basic grouping of the operative systems and sub 
systems are not limited to the descriptions below. 

The LOST-2 system uses a single position marker located at some known point. The 
position marker is acoustically coupled to one or more submersible vehicles. This allows 
the underwater vehicle's range from the position marker to be computed using an 
acoustic ranging device. Bathymetry data and dynamic data are also employed in concert 
with the range data to estimate and predict the underwater vehicle's position. 

The LOST-2 employs a system observer subsystem having a state velocity update 
module, a terrain matching module and means for generating a prediction of the 
underwater vehicles spatial performance. The second subsystem, the Kalman filter 
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subsystem comprises a constrained extended Kalman filter subsystem having a steady 
state extended Kalman filter, a non-linear constraint module, and a state predictor. 

The system observer integrates bathymetry data corresponding to the area of the 
submersible vehicle with the vessel's measured ocean depth, the vessel's predicted state, 
the vessel's measured velocity, and the measured slant range and location of a known 
point into a terrain based state estimate, a final predicted state and a measured slant range 
to the known point. This information is used by the second subsystem, the Kalman filter 
to generate a final estimate of the underwater vehicles position. 

Specifically, the Kalman filter takes the terrain based state estimate, the final predicted 
state, the measured slant range and the location of the known point and computes the 
final estimate of state and a prediction of the next system state to generate an estimate of 
the position of the underwater vessel. 

Referring now to the figures, Fig. 5 shows a general overview of the implementation of 
an example embodiment of the LOST 2 system using a single transponder equipped buoy 
as the position marker. In applications where high resolution bathymetry data is 
available, a technique exist for the measurement of the ocean depth 590 at the position of 
the underwater vehicle and a technique exist for the measurement of the slant range 560 
of the underwater vehicle to a position marker 520. The position marker must be easily 
related to a known point. A single point terrain match based on bathymetry data 550 for 
the local area is also used for underwater vehicle locaUzation. LOST 2 also uses the 
underwater vehicle's velocity data as well as other underwater vehicle dynamic 
information to estimate and predict the underwater vehicles position. In this example 
embodiment the underwater vehicle used is untethered, a feature allowed by the use of 2 
dimensional modeling discussed below. Briefly, the LOST 2 system employs a 2 
dimensional model addressing the underwater vehicle's longitudal track and the 
underwater vehicle's cross track. This feature allows LOST 2 to generate accurate 
position data when both the longitudal track as well as the cross track of the underwater 
vehicle vary. Via the use of 2 dimensional modeling (longitudal track and cross track) 
and a single position marker, the LOST 2 system may deploy and track the location of 
several untethered underwater vehicles simultaneously. 

The LOST 2 system requires several input parameters. These include the (1) geo- 
rectified, high resolution bathymetry 550 for the operational area collected prior to the 
underwater vehicles deployment , (2) the depth 530 of the underwater vehicle 510 
through the use of a pressure sensor or with an up looking acoustic ranging device, (3) 
the velocity of the underwater vessel 570 both course and speed (4) and the slant range 
560 to the position marker 520 via a acoustic ranging device mounted on the position 
marker or on the underwater vehicle itself The position marker is located a known point 
which is preferably a geo-rectified 580 point. The LOST 2 system's output is the 
estimated geo-rectified position of the underwater vehicle 510. 

Figure 6 shows the top view of an example embodiment a LOST 2 system operating and 
providing the position of 4 untethered underwater vehicles simultaneously 600. A single 
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position marker 620 having a known position is acoustically coupled to untethered 
underwater vehicles 610, 610', 610", and 610'". Preferably each underwater vehicle 
610 is equipped with a simple transponder which is acoustically coupled to position 
marker 620 which may be a floating buoy or located on the sea floor. By acoustic 
coupling to the position marker, the slant range of each underwater vehicle to the single 
known point 670, is measured. For the area in which each of the underwater vehicles 
operates there must exist geo-rectified, high resolution bathymetry data collected prior to 
the underwater vehicles deployment. When operating multiple underwater vehicles 
LOST 2 requires a depth measurement for each underwater vehicle 601 and the velocity 
of each of the underwater vessel's including both course and speed. 

Independently, for each underwater vehicle LOST-2 predicts the underwater vehicle's 
position, based on a past estimate of the underwater vehicle's position, and an estimate of 
its velocity over the sea bottom. LOST 2 then estimates the underwater vehicle's 
position utilizing measured ocean depth at the underwater vehicle's position, bathymetry 
data and the underwater vehicle's predicted position in a single point terrain match. The 
system then computes an estimate of the underwater vehicle's position based on the 
prediction of the imderwater vehicle's position based on vehicle dynamics and the 
estimated underwater vehicles position based on ocean depth and bathymetry data. 
LOST 2 then computes a corrected estimate of the submersible vehicle's position that 
utilizing the estimate of the underwater vehicle's position and a measured slant range 
from the underwater vehicle to the sea borne position marker whose position is known. 
These steps are repeated for each underwater vehicle using bathymetry data and dynamic 
information relevant to that particular underwater vehicle. 

Figure 7 shows yet another example embodiment of the LOST 2. This embodiment of 
the LOST 2 system also uses a single position marker 720 located on the sea floor 
acoustically coupled to underwater vehicle 710. Similar to the example embodiment 
shown in figure 5 the position marker is located at a known position, preferably the 
location of the position marker is geo-rectified. 

Figure 8 shows a block diagram of the algorithm employed by the LOST 2 system. The 
LOST 2 system comprises two basic subsystems, the system observer 810 and the 
constrained extended Kalman filter 820. These two subsystems are composed of six 
modules. The basic grouping of the operative modules and sub systems are not limited to 
the groupings as described above or below. 

The system observer subsystem is composed of three modules: a state 
velocity update 850, terrain matching module 840, and means for generating a prediction 
of the terrain mapping performance shown in this example as spatial performance 
prediction module 830. 

The constrained extended Kalman filter subsystem 820 consists of three modules: a 
steady state extended Kalman filter 870, a non-linear constraint module 880, and a state 
predictor 890. 



5 



System observer 810 takes in all the available measurements and generates two position 
estimates, one from Newtonian motion and the other based on the terrain in the area. As 
shown in Figure 8, system observer 810 integrates the high resolution bathymetry 811, 
vessel's measured ocean depth 812, vessel's predicted state 13, the vessel's measured 
velocity 814, and the measured slant range and location of the known point 815 into the 
terrain based state estimate 841. The system outputs the terrain based state estimate 841, 
a final predicted state 860 and the measured slant range and location of the known 
point. The system observer also computes a prediction of the performance of the terrain 
mapping system shown in figure 8 as a spatial performance estimate 83 1 . 

The constrained extended Kalman filter 820 takes the terrain based 
state estimate 841, a final predicted state 860, the measured slant range and location of 
the known point 815 and computes the final estimate of state 899 and a prediction of the 
system state at the next time step 898. 

The inputs the system requires are a detailed, high resolution 
bathymetric map in the operational area 81 1, a slant range to a known position 815, 
an estimate of the underwater vehicle's state 813, a measured or estimated velocity 814 
and the depth of the ocean at the position of the underwater vehicle 812. 

Referring again to the embodiment shown in figure 5, the high resolution 
bathymetric map in the operational area is shown as 550, the slant range to a known 
position 560 is effective the range from underwater vehicle 510 to position marker 520, 
an estimate of the underwater vehicle's state which is generated by the LOST 2 system 
(not shown), a measured or estimated velocity of the underwater vehicle 570 and the 
depth of the ocean 590 at the position of underwater vehicle 510. These inputs are used 
in conjunction with the output of the other modules to generate an accurate estimate of 
the position of the underwater vessel 510. 

Figure 9 shows a block diagram of the System Observer subsystem 910. As stated 
earlier, this subsystem's primary fionction is to generate all the predictions of the state 
that are required by the constrained extended Kalman filter. It also provides a prediction 
of performance based on the spatial distribution of the bathymetry. This subsystem 
integrates the high resolution bathymetry, vessel's measured ocean depth, vessel's 
predicted state, the vessel's measured velocity, and the measured slant range and 
location of the known point into the terrain based state estimate. The subsystem's 
outputs are the terrain based state estimate, the final predicted state and the 
measured slant range and location of the known point. 

The first subsystem component is the state velocity update, whose signal 
flow diagram is shown in figure 10. The state velocity update module 850 serves to 
provide an estimated location of the underwater body. It does this by combining the best 
available velocity 814 (speed and heading), and the best estimate of state 830 to produce 
the best estimate state prior to the terrain match. This module then outputs the predicted 
state 840 at the present time. 
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The output of the state velocity update 840 is sent to the terrain matching 
module, whose signal flow is shown as figure 11. The terrain matching module is the part 
of the system that serves as the independent system observation. The observation is 
preferably made by comparing the measured ocean depth 11 12 to the bathymetry 1111 in 
the operational area. This accomplished by means of a maximum likelihood estimator. 
The output of this module is the terrain based state estimate 1141, which is also the 
independent system observation. 

The final module of the system observer is the spatial based performance 
prediction. The signal flow is contained in figure 12. The spatial performance prediction 
module 1230 produces an estimate of how well the system is capable of performing in a 
given area 123 1 based on the high resolution bathymetry 1211 in the area of the 
underwater vehicle. 

The system observer generates two separate estimates of the position of the imderwater 
body. The first position estimate is generated in the velocity update. This 
position estimate is then used in conjunction with bathymetry to generate the 
terrain based position estimate. 

State Velocity Update module 

Figure 10 shows a block diagram of the signal flow of the state velocity update module. 
The state velocity update module receives the vessel's predicted state using all of the 
information before the current time step 1013, and the vessel's measured velocity 1014 
and computes the vessel's final predicted state 1060. The vessel's final predicted state is 
the underwater vehicle's position, Pp(«), based on a past estimate of the underwater 
vehicle's position, Pe(«-1) and an estimate of the underwater vehicle's velocity over the 
sea bottom. 

The velocity update module serves as the dead-reckoning system and computes 
an estimated position for the vessel in the along track, cross track coordinate 
system based on a measured velocity vector for the underwater vessel, and the 
best prediction of position of the underwater vessel fi-om the previous time step. 
The velocity update calculates the new position (along track, cross track) 
as two separate and independent processes, one in each direction. This is done 
throughout the system. 

For each direction, the position estimate is computed by adding the measured 
velocity, multiplied by time as appropriate, to the previous position estimate. 

The formulas used to compute each part of the estimate are: 
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M|n— 1 




(3.3) 




n\n~l 




(3.4) 



where the superscripts a and c correspond to along track and cross track and v is the 
measured velocity. The subscript n\nA refers to the estimate of the state (x) at time (n) 
given all information available at time n-1. Therefore, Xn|n-i is the best estimate of the 
position before the time n measurements, is the velocity times the time between 
samples at time n. The resulting prediction of position is provided to both the terrain 
matching module, and the constrained extended Kalman filter. This prediction is used 
by the terrain matching module to generate the second observation of position. 

Terrain based position estimate 

The terrain matching module receives the bathymetry data, a measurement of the ocean's 
depth at the vessel's position and the vessel's final predicted state and computes the 
terrain based state estimate with a single point position match. 

The terrain matching module takes as its inputs the position generated by the 
velocity update module, the measured ocean depth at the vessel's position and the 
high resolution bathymetry for the area of operation. The module then generates the best 
estimate of position based on maximum estimation principles. 

hi the preferred embodiment, a reasonably dense, spatially varying bathymetric data set 
in the neighborhood of the vessel is used. Each point in this bathymetric data set is a 
triple, a\ = (x/, yi, zi). Where x/, yt is the 2-dimensional position vector (eastings and 
northings) in meters and z/ is the measured depth in meters, hi a preferred embodiment 
this data has been previously collected however it may be collected concurrently with the 
underwater vehicle deployment. 

Given the dead reckoning estimate of vessel position Xn and the measured 

ocean depth at the vessel's position, z^, the 3 dimension position vector, a-n = (Xm ^w), can 

be formed using the same units as a\ after rotating coordinate systems. Without 

loss of generality, it is assumed that the ocean bottom depth is measured directly 

beneath the vessel to simplify this exposition; in practice it may be necessary to 

correct for vessel orientation, which is possible through a set of rotations. If the 

true location of the vessel and ocean bottom depth is denoted as an = (x, y, z), 

then the measurement error, defined as the difference between the true and dead 

reckoned positions, can be written as e = an - a-n . 

Viewed as an estimation problem, a maximum likelihood approach is used. 
That is, given several observations from a distribution with unknown parameters 
the value of the observation that maximizes the likelihood function is the best 
estimate of the parameters. The an and a-n are related through the likelihood 
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function (LF) based on a known or assumed error distribution,^. 
The LF can be expressed as: 

L((^\0tn ) =fe{0£i-Otn) (3.5) 

Given the estimated position a-n and the bathymetry in the area fa\} the 
most hkely location of the vessel constrained to be in the bathymetric data set 
can be determined as that a\ which maximizes L(a\\a-n ). When applied to the 
bathymetric data set {a\}, (3.5) produces a measure which can be viewed as the 
weighted distance from the estimated position, "xn. Selecting the bathymetry triple 
which minimizes this distance is the maximum likelihood estimate of position 
constrained to the bathymetric data set. 

The Terrain matching module employs the following relation: 

X{ai\aZ) = (a-i - a^f S-^(ai - a,;) (3.7) 

where as (alpha i) refers to data points from the bathymetry set, % (alpha n) is the 3D 
position vector formed by the measured depth at the position of the underwater vessel 
and the estimate of vessel position, (from the state velocity update) One assumes the 
error e is to be zero mean Gaussian with covariance E. 

Because E is a covariance matrix and it is positive semidefinite, the likelihood 
fimction in (3.7) is strictly non-negative with minimum 0 at ^3ti = a-n . The correct value 
for the covariance matrix varies depending on the nature of the bathymetric data being 
used. 

The matrix given by Eqn. 3.8 contains the assumed variances in the position for each of 
the 3 axes. 
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where Ua is the variance in the along track direction, Oc is the variance in the along track 
direction and is the variance in the z direction. The resulting position from the Terrain 
matching module is the (alpha i) that minimized Eqn. 3.7. Substituting this matrix into 
Equ. 3.7 and performing minimization generates the MLE's observation of position 
which is then passed to the constrained Kalman filter. 
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Three of the modules in the L0ST2 system forai the second subsystem of the L0ST2 
system, a constrained extended Kalman filter. Figure 13 shows a block diagram of the 
constrained extended Kalman filter 1320 subsystem. The Kalman filter subsystem 
comprises a steady state extended Kalman filter 1370, a non-linear constraint module 
1380, and a state predictor 1390. The steady state extended Kalman filter 1370 features 
nonlinear constraints that are placed on it to improve performance and stability given the 
poor nature of all models for underwater vessels. 

The signal flow diagram of the constrained Kalman filter is shown in figure 13, builds on 
Kalman filter principles, extended to nonlinear systems, to integrate all of the state 
estimates into a single final state estimate 1399. This subsystem takes as input 
the terrain based state estimate 1341, a final predicted state 1360, the measured slant 
range and location of the known point 1315 and computes the final estimate of state 1399 
and a prediction of the next system state 1398. 

The extended Kalman filter that takes the previous estimate of the underwater vehicle's 
position, Pe(t-l), and the maximum likelihood estimator's estimate of the underwater 
vehicle's position, MLE(t), and computes a linear Kalman filter position estimate at time 
(t), KPAT(t). 

The steady state Kalman filter, contained in figure 14, is the heart of the constrained 
extended Kalman filter subsystem 1320. Referring now to figure 14, this filter merges 
the two estimates of state in accordance with Kalman filter theory. This module takes as 
input the two estimates of state, the vessel's final predicted state, 1460 and the terrain 
based state estimate 1441, and outputs a Hnear combination of the two 1475. 

The steady state Kalman filter receives the vessel's final predicted state and a terrain 
based state estimate fi'om said system observer subsystem and computes the linear filter's 
state estimate therefi"om. 

After the prediction and observation steps of the Kalman filter are completed, the position 
estimates of the system observer must be merged. The terrain based observation (y) is 
merged with the prediction of the velocity update by means of a linear filter. The linear 
extended Kalman filter is the merging step of the constrained extended Kalman filter. 

This module works in two steps. First the error or innovations of the system are computed 
by subtracting the observation fi-om the prediction. The second step is to merge the 
innovations with the prediction via Kalman filter theory. 

The innovations are computed by differencing the position obtained from the terrain 
match module (y) from the estimate of position at time n given all information at time n- 
1 . In essence the innovations define the new information that is available at the present 
time step («). The innovations are computed in each of the two independent directions 
separately. These 'innovations' can be expressed as: 
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Vn = Vn-Kln-l (4.15) 

< = y:-Kin-i (4.16) 

After the innovations are defined, they are merged with the prediction in each direction 
by the Kalman gain K. This can be done in this manner due to the assumption that the 
system decouples into two independent systems. 

The best Kalman filter estimate of position in eqn 4.17 and 4.18 is obtained by adding a 
portion of the innovations to the estimate of position at time n given all information at 
time n-1. The 'portion' is determined by iST, the Kalman filter gain which is heuristically 
set for this system (extended Kalman fitler) vs. computed (standard Kalman filter). 
Again following the Kalman principles, this is expressed as: 



X 



n\n 



(4.17) 
(4.18) 



This module would in a normal extended Kahnan filter complete one cycle through the 
filter algorithm. However, due to the unique nature of underwater vessels and this system, 
nonlinear constraints have been added to improve the system's performance. Due to the 
errors in linearization and the lack of a good system model, the linear filter's output must 
be constrained. This is done in two stages. First, the amount of change fi*om the predicted 
state is thresholded. Secondly, the slant range is used to force the estimated location to 
be the correct distance fi-om the known point. The nonlinear constraints module forms 
the constrained part of the constrained extended Kalman filter. A signal flow diagram for 
the nonlinear constraints module is shown in figure 15. 

The non-linear constraint module 1580 receives the underwater vehicle's measured slant 
range and location of the known point 1515, and the vessel's final predicted state 1560 
fi-om the system observer subsystem, and the linear filter's state estimate 1575 and 
generates an estimate the vessel's final state 1599. 

The Kalman filter has two problems associated with it, each of which are addressed by a 
separate set of nonlinear constraints. The first set of constraints serves to cull outliers 
fi-om the system by means of a threshold. The threshold limits the distance that the 
system is allowed to change the predicted location. The second constraint is a slant range 
fi-om a known point. This serves to aid in convergence and to prevent divergence. 

The first set of constraints serves to cull the outliers fi-om the observation 

module. These outliers may cause unpredictable performance. This is accomplished 
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by comparing the difference between Xn|n-i and Xn|n in each direction and by hmiting its 
magnitude to a certain value. Basically, it limits the change in estimated position from 
the Kalman filter in each axis to € to prevent system divergence. This may be expressed 
as: 
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These limits have the effect of trading convergence time for a vast increase 
in stability. The unit € is meters and a rule of thumb for a nominal value is 0.1 
meters per second. This is scaled by the sampling rate. 



The other set of system constraints is the slant range correction. This 
corrects the estimated location of the vessel by forcing it to have the correct 
horizontal range (magnitude only) from the transponder. It does this by forming 
a line starting from the transponder and ending at the estimated vessel position. 
This line is then scaled by moving the estimated position along the line until the 
range is correct. This serves to increases convergence time by allowing for large 
movements in position and prevents divergence by preventing positions that are 
not realistic given the slant range information. 

The nonlinear constraints distinguish this filter from other Kalman filters and are an 
extension of extended Kalman filters dictated by the nature of the system. By introducing 
nonhnearities into the linear system model, the system predicts the real dynamics better. 
The output of this module completes the merging of new information into the predicted 
location and is the best estimate as to the vessel's position. 

The final module in the constrained extended Kalman filter is the state predictor, which is 
shown in figure 16. The state predictor module serves as part of the prediction stage of 
the Kalman filter. 

This module computes an estimated position for the vessel in the along track, cross track 
coordinate system 1698 based on previous position estimates (stored internally). 
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The state predictor 1690 receives the vessel's final estimated state 1699 from the non 
linear constraint module and generates a prediction of the vessel's state at the next time 
step 1698 using the data generated for the current time step. 

The state predictor thus acts as a range corrector that utilizes the linear Kahnan filter 
position estimate at time (t), KPAT(t), a sea borne position marker, and a measured slant 
range from the at least one submersible vehicle to the sea borne position marker and 
computes a final estimate of the at least one submersible vehicle's position. 

This module projects the state ahead by keeping an internal running estimate of the 
unmeasured velocities and adds this to the final estimated state. This is the first stage in 
preparing one of the state estimates for the next time step. 

The state predictor calculates the new position (along track, cross track) 
as two separate and independent processes, one in each direction. This is done 
throughout this system. For each direction, the position estimate is computed by adding a 
portion of the unmeasured estimated velocity in each direction. This is done by 
differencing the previous two position estimates and multiplying the result by l-/x. The 
value of /X is different in each direction and for each specific system, but a ballpark figure 
for the correct value is 0. 1 . 

This provides an estimate of the vessels position at the next time step given all 
information known now, and it also serves to incorporate any unmeasured velocity which 
can be expressed for each axis as: 

K+im = < + (4.21) 

Kn\n = < + (1 - A*2)« - (4.22) 

Note that x at n+1 |n at time step n is equivalent to x at n|n-l at the next time step which is 
the value used by the state velocity update module (with the measured velocity to 
determine the estimate of vessel position based upon dead-reckoning alone). 

Note that the difference between x at n and n-1 is effectively using the last two position 
estimates to generate an estimate of vessel velocity independent of the vessel velocity 
input used by the state velocity updater. The resulting position estimate is then fed 
forward as one of the required inputs of the system observer. 

Bathymetry data, position and velocity of the underwater vehicle over the sea bottom, 
ocean depth at the position of the underwater vehicle, the underwater vehicle's slant 
range to the position marker and the georectified location of the position marker may be 
stored in the memory or memory devices of a computer. The computer is also utilized to 
analytically determine the underwater vehicle's position using the bathymetry data, the 
slant range of the underwater vehicle to the position marker, position and velocity of the 
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underwater vehicle over the sea bottom as well as displaying and storing the computed 
underwater vehicle's position. This analytically determined information may be 
displayed on either a video monitor or a printing device. 

Although this invention has be described in relation to the exemplary embodiment's 
thereof, it is well understood by those skilled in the art that other variations and 
modifications can be affected on the preferred embodiment without departing from the 
scope and the spirit of the invention as set fourth in the claims: 
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